#include <rclcpp/rclcpp.hpp>

// MoveIt Servo 依赖
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

#include <geometry_msgs/msg/twist_stamped.hpp>
#include <tf2_ros/buffer.h>

#include <string>
#include <chrono>

using namespace std::chrono_literals;

namespace velocity_node {

static rclcpp::Node::SharedPtr node_;
static rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("velocity_publisher");

// 控制循环：定时发布一个示例速度指令
static void publishTwistCommand()
{
    // --- 在这里设置你想要的速度指令 ---
    // 示例：沿 Z 轴正方向匀速运动 0.1 m/s
    constexpr double TARGET_VELOCITY = 0.1; 
    constexpr const char* FRAME_ID = "base_link"; // 确保与 Servo 配置的 frame_id 一致

    auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
    msg->header.stamp = node_->now();
    msg->header.frame_id = FRAME_ID; 

    // 设置线速度
    msg->twist.linear.x = 0.0;
    msg->twist.linear.y = 0.0;
    msg->twist.linear.z = TARGET_VELOCITY; 
    
    // 保持角速度为 0
    msg->twist.angular.x = 0.0;
    msg->twist.angular.y = 0.0;
    msg->twist.angular.z = 0.0;

    twist_pub_->publish(std::move(msg));

    RCLCPP_INFO_THROTTLE(
        LOGGER, *node_->get_clock(), 500,
        "[VEL PUB] Publishing Z-velocity: %.3f m/s in frame: %s",
        TARGET_VELOCITY, FRAME_ID);
}

} // namespace velocity_node

int main(int argc, char** argv)
{
    using namespace velocity_node;

    rclcpp::init(argc, argv);
    rclcpp::NodeOptions opts;
    opts.use_intra_process_comms(false);
    node_ = std::make_shared<rclcpp::Node>("velocity_publisher", opts);

    // --- 1. MoveIt Planning Scene Monitor (Servo 依赖) ---
    auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
    auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
        node_, "robot_description", tf_buffer, "planning_scene_monitor");
    if (psm->getPlanningScene()) {
        psm->startStateMonitor("/joint_states");
        psm->setPlanningScenePublishingFrequency(25);
        psm->startPublishingPlanningScene(
            planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
            "/moveit_servo/publish_planning_scene");
        psm->startSceneMonitor();
        psm->providePlanningSceneService();
    } else {
        RCLCPP_ERROR(LOGGER, "Planning scene 未正确配置");
        return EXIT_FAILURE;
    }

    // --- 2. MoveIt Servo (必须创建并 start) ---
    auto servo_params = moveit_servo::ServoParameters::makeServoParameters(node_);
    auto servo = std::make_unique<moveit_servo::Servo>(node_, servo_params, psm);
    servo->start();

    // --- 3. 速度发布器 ---
    // 注意：Servo 默认监听 ~/delta_twist_cmds，这是正确的
    twist_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>(
        "~/delta_twist_cmds", 10);

    // --- 4. 控制循环：每 10ms 发布一次 ---
    auto timer = node_->create_wall_timer(10ms, &publishTwistCommand);

    // 使用多线程执行器
    auto exec = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
    exec->add_node(node_);
    exec->spin();

    rclcpp::shutdown();
    return 0;
}